/*
多目标坐标系转换：world坐标系下有两个坐标系，son1和son2.已知son1和son2的坐标点。
1.求son1在son2下的坐标。
2.求son1中的一个点在son2下的坐标。
代码分析：
1.son1和son2的世界坐标，已经通过tfs_c.launch文件发布
2.这里只需要实现订阅方，来实现这个两个需求
*/
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
// #include "geometry_msgs/TransformStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "tfs_sub_node");
    ros::NodeHandle nh;
    ros::Rate rate(3);
    //创建son1下的一个坐标点
    geometry_msgs::PointStamped psAtson1;
    psAtson1.header.frame_id = "son1";
    psAtson1.point.x = 1.0;
    psAtson1.point.y = 2.0;
    psAtson1.point.z = 3.0;

    //创建消息接收对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    while (ros::ok())
    {
        try
        {
            // 1.计算son1相对于son2的位置
            geometry_msgs::TransformStamped son1Toson2 = buffer.lookupTransform("son2", "son1", ros::Time(0));
            ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s", son1Toson2.header.frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s", son1Toson2.child_frame_id.c_str());
            ROS_INFO("son1相对于son2的坐标为:(%.2f,%.2f,%.2f).",
                     son1Toson2.transform.translation.x,
                     son1Toson2.transform.translation.y,
                     son1Toson2.transform.translation.z);

            // 2.计算son1下的点在son2坐标系下的位置
            geometry_msgs::PointStamped psAtson2 = buffer.transform(psAtson1, "son2");
            ROS_INFO("点在son2中的坐标为:(%.2f,%.2f,%.2f)", psAtson2.point.x, psAtson2.point.y, psAtson2.point.z);
        }
        catch (const std::exception &e)
        {
            ROS_INFO("错误提示:%s", e.what());
        }
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}
